#include <iostream>
#include <fstream>
#include <cmath>
#include <sstream>
#include <cstring>
#include <vector>
#include <time.h>
#include <stdio.h>


#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_errno.h>

#define QUADRANTES 2*M_PI
#define pi M_PI


using namespace std;

struct NODE_RWP{
    double x,y,s,waypointX,waypointY,steps;

    NODE_RWP(){
    }
    NODE_RWP(double x,double y,double s, double waypointX, double waypointY, double steps)
    {
        this->x =x;
        this->y=y;
        this->s=s;
        this->waypointX = waypointX;
        this->waypointY = waypointY;
        this->steps = steps;
    }
};
struct NODE_UNI{
    double x,y,distance,direction,speed;
    NODE_UNI(){
    }
    NODE_UNI(double x,double y){
        this->x=x;
        this->y-y;
    }
    NODE_UNI(double x,double y,double distance, double direction, double speed){
        this->x =x;
        this->y=y;
        this->distance=distance;
        this->direction = direction;
        this->speed = speed;
    }
};

struct NODE_RGM{
    double x,y,direction,speed,speed_medio,direction_medio;
    NODE_RGM(){
    }
    NODE_RGM(double x,double y,double direction, double speed,double direction_medio,double speed_medio){
        this->x=x;
        this->y=y;
        this->direction= direction;
        this->speed=speed;
        this->speed_medio=speed_medio;
        this->direction_medio=direction_medio;
    }

};

const double X=10,Y=10,MINSPEED=0,MAXSPEED=20,PAUSETIME=0;
const long AMOSTRAS=30,N=50,TIME=100000;
time_t start, finish;
const gsl_rng_type *rng_type;
gsl_rng *rng;

void RWP(ofstream &output){

	gsl_rng_env_setup();
    rng_type = gsl_rng_default;
    rng = gsl_rng_alloc (rng_type);
    gsl_rng_set(rng,time(NULL));


	NODE_RWP nodes[N];

    vector<float> velocidades (TIME);
    vector< vector<float> > velocidades_media(AMOSTRAS,TIME);
    vector<float> velocidades_media_amostra(TIME);

    double x,y,new_x,new_y,waypointX,waypointY,s,deltax,deltay,dist,steps,alfa,sum_vel,sum_amos;

    for (int k=0;k<AMOSTRAS;k++){
        time(&start);
        for (int i=0;i<N;i++){

            x = X*gsl_rng_uniform(rng);
            y = Y*gsl_rng_uniform(rng);
            waypointX = gsl_rng_uniform(rng)*X;
            waypointY = gsl_rng_uniform(rng)*Y;
            s = MINSPEED + ((MAXSPEED-MINSPEED) * gsl_ran_beta(rng,2,2)) ;
            deltax = waypointX - x;
            deltay = waypointY - y;
            dist   = sqrt(pow(deltax,2)+pow(deltay,2));
            steps = dist/s;
            NODE_RWP node(x, y, s, waypointX, waypointY, steps);
            nodes[i] = node;

        }
        for (int t=0;t<TIME;t++){

            for (int i=0;i<N;i++){
                NODE_RWP node = nodes[i];
                NODE_RWP new_node;
                steps = node.steps;
                if (steps>=1){
                    x = node.x;
                    y = node.y;
                    s = node.s;
                    waypointX = node.waypointX;
                    waypointY = node.waypointY;

                    deltax = waypointX - x;
                    deltay = waypointY - y;

                    alfa = atan(deltay/deltax);
                    if ((deltax<0 && deltay>0) || (deltax<0 && deltay<0))
                        alfa = M_PI + alfa;
                    else if (x>0 && y<0)
                        alfa = 2*M_PI + alfa;

                    new_x = x + s*cos(alfa);
                    new_y = y + s*sin(alfa);
                    steps = steps - 1;
                    new_node = NODE_RWP(new_x,new_y,s,waypointX,waypointY,steps);
                }else{
                    node.x = node.waypointX;
                    node.y = node.waypointY;
                    x = node.x;
                    y = node.y;
                    waypointX = gsl_rng_uniform(rng)*X;
                    waypointY = gsl_rng_uniform(rng)*Y;
                    s = MINSPEED + ((MAXSPEED-MINSPEED) * gsl_ran_beta(rng,2,2));

                    deltax = waypointX - x;
                    deltay = waypointY - y;
                    dist   = sqrt(pow(deltax,2)+pow(deltay,2));
                    steps = dist/s;

                    new_node = NODE_RWP(node.x,node.y,s,waypointX,waypointY,steps);
                }
                //cout << s<<endl;
                nodes[i] = new_node;
            }
            sum_vel = 0;
            for (int i=0;i<N;i++){

                 sum_vel += nodes[i].s;

            }
            velocidades[t] = sum_vel/N;

            //   velocidades(:,t) =  nodes(:,3); velocidades_media[AMOSTRAS][TIME]
        }
        for (int t=0;t<TIME;t++)
            velocidades_media[k][t] = velocidades[t];
        time(&finish);
        cout <<"Tempo Amostra ("<<k<<"): "<<difftime(finish,start)<<" segundos."<<endl;
    }

    output << "velocidades_rwp = ["<<endl;
    for (int t=0;t<TIME;t++){
        sum_amos = 0;
        for (int k=0;k<AMOSTRAS;k++)
            sum_amos +=velocidades_media[k][t];
        velocidades_media_amostra[t] = sum_amos/AMOSTRAS;

       // cout<< sum_amos << endl;
        output << velocidades_media_amostra[t] << " ";
    }
    output << "];"<<endl;

}

void Uniforme(double k2,ofstream &output){

    gsl_rng_env_setup();
    rng_type = gsl_rng_default;
    rng = gsl_rng_alloc (rng_type);
    gsl_rng_set(rng,time(NULL));


    double x,y,speed,direction,distance,mu=k2/N,deltax,deltay,a,b,sum_vel,sum_amos;

    NODE_UNI nodes[N];

    vector<float> velocidades (TIME);
    vector< vector<float> > velocidades_media(AMOSTRAS,TIME);
    vector<float> velocidades_media_amostra(TIME);

    for (int k=0;k<AMOSTRAS;k++){
        time(&start);
        for (int i=0;i<N;i++){
            x = X*gsl_rng_uniform(rng);
            y = Y*gsl_rng_uniform(rng);
            distance = gsl_ran_exponential (rng, mu);
            direction = QUADRANTES*gsl_rng_uniform(rng);
            speed = MINSPEED + ((MAXSPEED-MINSPEED) * gsl_ran_beta(rng,2,2)) ;
            NODE_UNI node(x, y, distance, direction,  speed);
            nodes[i] = node;

        }
        for (int t=0;t<TIME;t++){

            for (int i=0;i<N;i++){

                NODE_UNI node = nodes[i];
                NODE_UNI nodeAnterior = node;
                speed = node.speed;
             //   cout << node.speed << endl;
                node.x = node.x + speed*cos(node.direction);
                node.y = node.y + speed*sin(node.direction);
                NODE_UNI reflexPoint;

                while (node.y<0 || node.y>Y || node.x<0 || node.x>X){
                    deltax=node.x-nodeAnterior.x;
                    deltay=node.y-nodeAnterior.y;
                    a = deltay/deltax;
                    b = node.y - (a*node.x);
                    if(node.y<0){
                        NODE_UNI lastPoint(((0 - b)/a), 0);
                        NODE_UNI reflexPoint(node.x, (-1*node.y));
                        node.y = -1*node.y;

                        deltax=reflexPoint.x-lastPoint.x;
                        deltay=node.y-nodeAnterior.y;

                        if(node.direction>3*pi/2 && node.direction<2*pi)
                            node.direction =  atan(deltay/deltax);
                        if(node.direction>pi && node.direction<3*pi/2)
                            node.direction = pi + atan(deltay/deltax);

                    }else{
                        if(node.y>Y){
                            NODE_UNI lastPoint(((Y - b)/a), Y);
                            NODE_UNI reflexPoint(node.x, (2*Y - node.y));
                            node.y = 2*Y - node.y;

                            deltax=reflexPoint.x-lastPoint.x;
                            deltay=node.y-nodeAnterior.y;

                            if(node.direction>0 && node.direction<pi/2)
                                node.direction = 2*pi + atan(deltay/deltax);
                            if(node.direction>pi/2 && node.direction<pi)
                                node.direction = pi+ atan(deltay/deltax);
                        }else{
                            if(node.x<0){
                                NODE_UNI lastPoint(0, (a*0 + b));
                                NODE_UNI reflexPoint((-1*node.x), node.y);
                                node.x = -1*node.x;

                                deltax=reflexPoint.x-lastPoint.x;
                                deltay=node.y-nodeAnterior.y;

                                if(node.direction>pi && node.direction<3*pi/2)
                                    node.direction = 2*pi + atan(deltay/deltax);
                                if(node.direction>pi/2 && node.direction<pi)
                                    node.direction =  atan(deltay/deltax);
                            }else{
                                NODE_UNI lastPoint(X ,(a*X + b));
                                NODE_UNI reflexPoint((2*X - node.x), node.y);
                                node.x = 2*X - node.x;

                                deltax=reflexPoint.x-lastPoint.x;
                                deltay=node.y-nodeAnterior.y;

                                if(node.direction>0 && node.direction<pi/2)
                                    node.direction =  pi + atan(deltay/deltax);
                                if(node.direction>3*pi/2 && node.direction<2*pi)
                                    node.direction = pi + atan(deltay/deltax);

                            }
                        }
                    }
                }
                node.distance = node.distance-speed ;

                if(node.distance<=0){
                    distance = gsl_ran_exponential (rng, mu);
                    direction = QUADRANTES*gsl_rng_uniform(rng);
                    speed = MINSPEED + ((MAXSPEED-MINSPEED) *gsl_ran_beta(rng,2,2)) ;
                    node.distance = distance;
                    node.direction = direction;
                    node.speed = speed;

                }
                nodes[i] = node;

                sum_vel = 0;
                for (int i=0;i<N;i++)
                    sum_vel += nodes[i].speed;
                velocidades[t] = sum_vel/N;
            }
        }
        for (int t=0;t<TIME;t++)
            velocidades_media[k][t] = velocidades[t];
        time(&finish);
        cout <<"Tempo Amostra ("<<k<<"): "<<difftime(finish,start)<<" segundos."<<endl;


    }


    output << "velocidades_uniforme = ["<<endl;
    for (int t=0;t<TIME;t++){
        sum_amos = 0;
        for (int k=0;k<AMOSTRAS;k++)
            sum_amos +=velocidades_media[k][t];
        velocidades_media_amostra[t] = sum_amos/AMOSTRAS;

       // cout<< sum_amos << endl;
        output << velocidades_media_amostra[t] << " ";
    }
    output << "];"<<endl;
}

/*
void RGM(double a,double borda){

    gsl_rng_env_setup();
    rng_type = gsl_rng_default;
    rng = gsl_rng_alloc (rng_type);
    gsl_rng_set(rng,time(NULL));

	NODE_RGM nodes[N];

    vector<float> velocidades (TIME);
    vector< vector<float> > velocidades_media(AMOSTRAS,TIME);
    vector<float> velocidades_media_amostra(TIME);

    double x,y,direction,speed,speed_medio,direction_medio;

     for (int k=0;k<AMOSTRAS;k++){
        time(&start);
        for (int i=0;i<N;i++){
            x=borda + gsl_rng_uniform(rng)*(X-2*borda);
            y=borda + gsl_rng_uniform(rng)*(Y-2*borda);
            direction=QUADRANTES*gsl_rng_uniform(rng);
            speed = MINSPEED + ((MAXSPEED-MINSPEED) * gsl_rng_uniform(rng)) ;
            direction_medio = pi/2
            speed_medio =3;
            NODE_RGM node(x, y,direction, speed, direction_medio, speed_medio);
            nodes[i] = node;
        }

        for (int t=0;t<TIME;t++){

            for (int i=0;i<N;i++){
                NODE_RGM node = nodes[i];

                x = node.x;
                y = node.y;
                if (x > 0 && x < borda && y > 0 && y < borda)
                    direction_medio = pi/4;
                else if x > borda && x < X - borda && x > 0 && y < borda
                    direction_medio = pi/2;
                else if x > X - borda && x < X && y > 0 && y < borda
                    direction_medio = 3*pi/4;
                else if x > X - borda && x < X && y > borda  && y < Y - borda
                    direction_medio= pi;
                else if x > X - borda && x < X && y > Y - borda  && y < Y
                    direction_medio= 5*pi/4;
                else if x > borda && x < X - borda && y > Y - borda  && y < Y
                    direction_medio = 3*pi/2;
                else if x > 0 && x < borda && y > Y - borda  && y < Y
                    direction_medio = 7*pi/4;
                else if x > 0 && x < borda && y > borda  && y< Y - borda
                    direction_medio = 0;
                else
                    direction_medio = node.direction_medio;

                NODE_RGM oldnode = nodes_anterior(i,:);
                oldspeed = oldnode(3) ;
                olddirection = oldnode(4);
                s_ = oldnode(5);
                d_ = oldnode(6);
                newspeed = a*oldspeed + (1-a)*s_ + sqrt(1-a^2)*normrnd(0,1);
                newdirection =a*olddirection + (1-a)*d_ + sqrt(1-a^2)*normrnd(0,1);
                newnode = [oldnode(1:2) newspeed newdirection oldnode(5:6)];
                nodes(i,:) = newnode;


            }

        }


     }
}

void Group(){
}
*/
int main()
{
    ofstream output;
    output.open("output.m");

    RWP(output);
    Uniforme(300,output);

    output.close();

    return 0;

}
